#include"fast_lio/utility.h"

namespace fast_lio{

sensor_msgs::Imu imuConverter2Lidar(const sensor_msgs::Imu& imu_in, const Config::Ptr p_config){
    sensor_msgs::Imu imu_out = imu_in;
    // rotate acceleration
    Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
    acc = p_config->ext_imu_lidar_rot_.inverse() * acc;
    imu_out.linear_acceleration.x = acc.x();
    imu_out.linear_acceleration.y = acc.y();
    imu_out.linear_acceleration.z = acc.z();
    // rotate gyroscope
    Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
    gyr = p_config->ext_imu_lidar_rot_.inverse() * gyr;
    imu_out.angular_velocity.x = gyr.x();
    imu_out.angular_velocity.y = gyr.y();
    imu_out.angular_velocity.z = gyr.z();

    return imu_out;
}

Eigen::Matrix4d SE3toEigenM(const SE3 &pose){
    Eigen::Matrix4d T_w = Eigen::Matrix4d::Identity();
    T_w.block<3, 3>(0, 0) = pose.rotationMatrix();
    T_w.block<3, 1>(0, 3) = pose.translation();
    return T_w;
}

void SE3toRt(const SE3 &pose,double* var){
    Eigen::Quaterniond q(pose.rotationMatrix());
    Eigen::Vector3d trans(pose.translation());
    var[0] = q.x();
    var[1] = q.y();
    var[2] = q.z();
    var[3] = q.w();
    var[4] = trans[0];
    var[5] = trans[1];
    var[6] = trans[2];
}

SE3 Rt2SE3(double* var){
    return SE3(Eigen::Quaterniond(var[3],var[0],var[1],var[2]),Eigen::Vector3d(var[4],var[5],var[6]));
}

double compute_inlier_residual_threshold(std::vector<double> residuals, double ratio)
{
    std::set<double> dis_vec; // 升序
    for (size_t i = 0; i < (size_t)(residuals.size() / 3); i++)
    {
        dis_vec.insert(fabs(residuals[3 * i + 0]) + fabs(residuals[3 * i + 1]) + fabs(residuals[3 * i + 2]));
    }

    return *(std::next(dis_vec.begin(), (int)((ratio)*dis_vec.size())));
}

Eigen::Matrix<double, 3, 1> pcl_pt_to_eigend(PointType &pt)
{
    return Eigen::Matrix<double, 3, 1>(pt.x, pt.y, pt.z);
}

void pcDownSample(pcl::PointCloud<PointType>::Ptr pc, double size){
    pcl::VoxelGrid<PointType> dw;
    dw.setInputCloud(pc);
    dw.setLeafSize(size,size,size);
    dw.filter(*pc);
}

sensor_msgs::PointCloud2 publishCloud(ros::Publisher& thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
{
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);
    tempCloud.header.stamp = thisStamp;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub.getNumSubscribers() != 0)
        thisPub.publish(tempCloud);
    return tempCloud;
}

void cout_array(double* var,int size){
    cout<<"var ";
    for(int i = 0; i<size; i++){
        cout<<var[i]<<" ";
    }
    cout<<endl;
}

void anti_symmetric(Eigen::Vector3d const &_v, Eigen::Matrix3d &_m)
{
    _m(0, 0) = 0.0; _m(0, 1) = -_v.z(); _m(0, 2) = _v.y();
    _m(1, 0) = _v.z();_m(1, 1) = 0.0;_m(1, 2) = -_v.x();
    _m(2, 0) = -_v.y();_m(2, 1) = _v.x();_m(2, 2) = 0.0;
}

void tranformPoint(const PointType p_in,PointType & p_out, Eigen::Matrix4d T){
    Eigen::Vector4d p_v(p_in.x,p_in.y,p_in.z,1);
    p_v = T*p_v;
    p_out.x = p_v[0];
    p_out.y = p_v[1];
    p_out.z = p_v[2];
}

double pointDistance(PointType p)
{
    return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}

// gtsam::Pose3 SE3togtsamPose(const SE3& pose){
//     return gtsam::Pose3(gtsam::Quaternion(pose.rotationMatrix()),pose.translation());
// }

// SE3 gtsamPose2SE3(const gtsam::Pose3 pose){
//     return SE3(pose.rotation().toQuaternion(),pose.translation());
// }

Eigen::Matrix3d rightJacobiansInv(V3D fi){
    double theta = fi.norm();
    V3D a = fi.normalized();
    double term1 = 0.5*theta*(1.0/tan(0.5*theta));
    Eigen::Matrix3d ret = Eigen::Matrix3d::Zero();
    ret = term1*Eigen::Matrix3d::Identity()+
                (1-term1)*a*a.transpose() + 
                0.5*theta*Sophus::SO3d::hat(a);
    return ret;
}

void enforceSymmetry(Eigen::MatrixXd &mat) {
  mat = 0.5 * (mat + mat.transpose()).eval();
}

Eigen::Matrix3d vecToMatrix(const V3D & v){
    Eigen::Matrix3d res;
    double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
	double norm = std::sqrt(squaredNorm);
	if(norm < 1e-11){
		res = Eigen::Matrix<double, 3, 3>::Identity();
	}
	else{
		res = Eigen::Matrix<double, 3, 3>::Identity() + 
            (1 - std::cos(norm)) / squaredNorm * Sophus::SO3d::hat(v) + 
            (1 - std::sin(norm) / norm) / squaredNorm * Sophus::SO3d::hat(v) * Sophus::SO3d::hat(v);
	}
    return res;
}

}
